#include <assert.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "scorbot.h"

poDesc_t makePoFromScorbotXYZPR(double X, double Y, double Z, double P, double R){
	poDesc_t po;
	double arm_offset, offset_theta, pi;
	pi = M_PI;
	arm_offset = 325.0;
	
	po.x = X; //from 0.1 millimeters to millimeters
	po.y = Y;
	po.z = Z;
	po.phi = -((P/10)*(pi/180)); //from 0.1 degrees to radians
	po.rho = -((R/10)*(pi/180));
	offset_theta = asin(arm_offset / (sqrt(pow(po.x,2) + pow(po.y,2))));
	po.theta = atan2(po.y, po.x) + offset_theta; //get yaw from y and x
	
	return po;
}

void makeHB2GFromPo(poDesc_t po, H_Matrix *h_b2g){
	
	double cp, cr, ct, sp, sr, st, nx, ny, nz;
	nx = -po.x;
	ny = -po.y;
	nz = -po.z;
	
	cp = cos(po.phi);
	cr = cos(po.rho);
	ct = cos(po.theta);
	sp = sin(po.phi);
	sr = sin(po.rho);
	st = sin(po.theta);
	
	//rotation		//TRANSPOSED!
	h_b2g->e[0][0] = ct*cp;
	h_b2g->e[1][0] = ct*sp*sr-st*cr;
	h_b2g->e[2][0] = ct*sp*cr+st*sr;
	h_b2g->e[0][1] = st*cp;
	h_b2g->e[1][1] = st*sp*sr+ct*cr;
	h_b2g->e[2][1] = st*sp*cr-ct*sr;
	h_b2g->e[0][2] = -sp;
	h_b2g->e[1][2] = cp*sr;
	h_b2g->e[2][2] = cp*cr;
	
	//translation
	h_b2g->e[0][3] = h_b2g->e[0][0]*nx+h_b2g->e[0][1]*ny+h_b2g->e[0][2]*nz;
	h_b2g->e[1][3] = h_b2g->e[1][0]*nx+h_b2g->e[1][1]*ny+h_b2g->e[1][2]*nz;
	h_b2g->e[2][3] = h_b2g->e[2][0]*nx+h_b2g->e[2][1]*ny+h_b2g->e[2][2]*nz;
	
	//last row
	h_b2g->e[3][0] = 0;
	h_b2g->e[3][1] = 0;
	h_b2g->e[3][2] = 0;
	h_b2g->e[3][3] = 1;

}

void makeHG2BFromHB2G(H_Matrix *h_b2g, H_Matrix *h_g2b){
	
	//rotation
	h_g2b->e[0][0] = h_b2g->e[0][0];
	h_g2b->e[0][1] = h_b2g->e[1][0];
	h_g2b->e[0][2] = h_b2g->e[2][0];
	h_g2b->e[1][0] = h_b2g->e[0][1];
	h_g2b->e[1][1] = h_b2g->e[1][1];
	h_g2b->e[1][2] = h_b2g->e[2][1];
	h_g2b->e[2][0] = h_b2g->e[0][2];
	h_g2b->e[2][1] = h_b2g->e[1][2];
	h_g2b->e[2][2] = h_b2g->e[2][2];
	
	//translation
	h_g2b->e[0][3] = -(h_g2b->e[0][0]*h_b2g->e[0][3]+h_g2b->e[0][1]*h_b2g->e[1][3]+h_g2b->e[0][2]*h_b2g->e[2][3]);
	h_g2b->e[1][3] = -(h_g2b->e[1][0]*h_b2g->e[0][3]+h_g2b->e[1][1]*h_b2g->e[1][3]+h_g2b->e[1][2]*h_b2g->e[2][3]);
	h_g2b->e[2][3] = -(h_g2b->e[2][0]*h_b2g->e[0][3]+h_g2b->e[2][1]*h_b2g->e[1][3]+h_g2b->e[2][2]*h_b2g->e[2][3]);
	
	//last row
	h_g2b->e[3][0] = 0;
	h_g2b->e[3][1] = 0;
	h_g2b->e[3][2] = 0;
	h_g2b->e[3][3] = 1;
	
}


/*
int main(){
	
	double X, Y, Z, P, R;
	poDesc_t po;
	H_Matrix h_b2g, h_g2b;
	
	printf("%s", "\nType X: \n");
	scanf("%le", &X);
	printf("%s", "\nType Y: \n");
	scanf("%le", &Y);	
	printf("%s", "\nType Z: \n");
	scanf("%le", &Z);
	printf("%s", "\nType P: \n");
	scanf("%le", &P);	
	printf("%s", "\nType R: \n");
	scanf("%le", &R);
	
	po = makePoFromScorbotXYZPR(X, Y, Z, P, R);
	makeHB2GFromPo(po, &h_b2g);
	makeHG2BFromHB2G(&h_b2g, &h_g2b);
	
	printf("%s", "\n\n[");
	printf("%le", h_g2b.e[0][0]);
	printf("%s", " ");
	printf("%le", h_g2b.e[0][1]);
	printf("%s", " ");
	printf("%le", h_g2b.e[0][2]);
	printf("%s", " ");
	printf("%le", h_g2b.e[0][3]);
	printf("%s", ";");
	printf("%le", h_g2b.e[1][0]);
	printf("%s", " ");
	printf("%le", h_g2b.e[1][1]);
	printf("%s", " ");
	printf("%le", h_g2b.e[1][2]);
	printf("%s", " ");
	printf("%le", h_g2b.e[1][3]);
	printf("%s", ";");
	printf("%le", h_g2b.e[2][0]);
	printf("%s", " ");
	printf("%le", h_g2b.e[2][1]);
	printf("%s", " ");
	printf("%le", h_g2b.e[2][2]);
	printf("%s", " ");
	printf("%le", h_g2b.e[2][3]);
	printf("%s", ";");
	printf("%le", h_g2b.e[3][0]);
	printf("%s", " ");
	printf("%le", h_g2b.e[3][1]);
	printf("%s", " ");
	printf("%le", h_g2b.e[3][2]);
	printf("%s", " ");
	printf("%le", h_g2b.e[3][3]);
	printf("%s", "]\n");
	
	return 0;
}
*/
